#!/usr/bin/env python2.7
# -*- coding: utf-8 -*-
import rospy

import sys
from rospkg import RosPack
sys.path.append(RosPack().get_path('canopen_communication') + "/modular/")
from modular_T85 import T85

from time import sleep

def main():
    eds_file = RosPack().get_path('canopen_communication') + "/file/Copley.eds"
    # 关节初始化，eds_file为CANOpen相应配置文件
    joint = T85(5, eds_file)
    # 启动关节
    joint.start()
    # 设置关节运动模式
    joint.opmode_set('PROFILED POSITION')


    position = 30
    velocity = 5 

    # 给对应的关节发送位置与速度
    joint.sent_position(position, velocity)
    sleep(2)
    print "pause run"
    joint.pause_run()
    sleep(2)
    print "continue run"
    joint.continue_run()
    sleep(2)
 
    joint.stop()
    joint.stop_communication()

if __name__ == "__main__":
    main()     

